#include "car.h"

#include "hongwai.h"
#include "motors.h"
#include "BlueTooth.h"

CAR_MODE_E car_mode = MODE_PC;
CAR_MOVE_E car_move = CAR_STOP;


static void car_follow(void)
{
    LINE_POS_E line_pos = line_scan();
    switch (line_pos)
    {
    case LINE_M:
        Motor_Forward();
        printf("mid\r\n");
        break;
    case LINE_R:
        Motor_GoRight();
        printf("right\r\n");
        break;
    case LINE_L:
        Motor_GoLeft();
         printf("left\r\n");
        break;
    
    default:
        break;
    }
}

static void cmd_scan(void)
{
    switch (recv_data)
    {
    case 'f':
        car_mode = MODE_FOLLOW;
        printf("f\r\n");
        break;
    case 'p':
        car_mode = MODE_PC;
        Motor_Stop();
        printf("p\r\n");
        break;
    case 'w':
        Motor_Forward();
        printf("w\r\n");
        break;
    case 's':
        Motor_Back();
        printf("s\r\n");
        break;
    case 'a':
        Motor_GoLeft();
        printf("a\r\n");
        break;
    case 'd':
        Motor_GoRight();
        printf("d\r\n");
        break;
    case 'g':
        Motor_Stop();
        printf("g\r\n");
        break;
    
    default:
        break;
    }
}

void car_ctrl_loop(void)
{
    cmd_scan();
    if(car_mode == MODE_FOLLOW){
        car_follow();
    }
}